#define motores 1
#define MIR PIN_D5
#define MIF PIN_D4
#define MDR PIN_D7
#define MDF PIN_D6

void motores_patras()
{
      output_bit(MDF,1);
      output_bit(MDR,0);
      output_bit(MIF,1);
      output_bit(MIR,0);
}

void motores_palante()
{
      output_bit(MDF,0);
      output_bit(MDR,1);
      output_bit(MIF,0);
      output_bit(MIR,1);
}

void motores_paderecha()
{
      output_bit(MDF,1);
      output_bit(MDR,0);
      output_bit(MIF,0);
      output_bit(MIR,1);
}

void motores_paizda()
{
      output_bit(MDF,0);
      output_bit(MDR,1);
      output_bit(MIF,1);
      output_bit(MIR,0);
}

void motores_parar()
{
      output_bit(MDF,0);
      output_bit(MDR,0);
      output_bit(MIF,0);
      output_bit(MIR,0);
}



void motores_comprobar()
{
   printf (usb_cdc_putc, "Comprobando motores...\r\n");
   printf (usb_cdc_putc, "\tAdelante\r\n");
   motores_palante();
   delay_ms(2000);
   printf (usb_cdc_putc, "\tAtras\r\n");
   motores_patras();
   delay_ms(2000);
   printf (usb_cdc_putc, "\tDerecha\r\n");
   motores_paderecha();
   delay_ms(2000);
   printf (usb_cdc_putc, "\tIzquierda\r\n");
   motores_paizda();
   delay_ms(2000);
   printf (usb_cdc_putc, "Motores comprobados\r\n\r\n");
}

int motores_cv_estado=0;
int motores_cv_velocidad=0;
int motores_cv_contador=0;
   
void motores_cv_off()
{
   setup_timer_2 ( T2_DISABLED, 0xff, 16);
   disable_interrupts(int_timer2);
   printf (usb_cdc_putc, "-TIMER2 [OFF]\r\n");
   printf (usb_cdc_putc,"-Control de velocidad [OFF]\r\n");
}

void motores_cv_configurar()
{
   setup_timer_2 ( T2_DIV_BY_16, 0x50, 1);
   enable_interrupts(int_timer2);
   printf (usb_cdc_putc, "-TIMER2 [ON]\r\n");
   printf (usb_cdc_putc,"-Control de velocidad [ON]\r\n");
}


#int_timer2
void motores_cv_int()
{
   if (motores_cv_contador>motores_cv_velocidad) 
   {
		motores_parar();
   }
   if (motores_cv_contador>254) 
   {
      motores_cv_contador=0;
      switch (motores_cv_estado) 
      {
         case 1:
            motores_palante();
            break;
         case 2:
            motores_patras();
            break;
         case 3:
            motores_paderecha();
            break;
         case 4:
            motores_paizda();
            break;
         case 0:
            motores_parar();
            break;
      }
   } else {
		motores_cv_contador++;
   }
}

void motores_cv_palante(int velocidad)
{
   motores_cv_velocidad=velocidad;
   motores_cv_estado=1;
}

void motores_cv_patras(int velocidad)
{
   motores_cv_velocidad=velocidad;
   motores_cv_estado=2;
}

void motores_cv_paderecha(int velocidad)
{
   motores_cv_velocidad=velocidad;
   motores_cv_estado=3;
}

void motores_cv_paizda(int velocidad)
{
   motores_cv_velocidad=velocidad;
   motores_cv_estado=4;
}

void motores_cv_parar()
{
   motores_cv_velocidad=0;
   motores_cv_estado=0;
}

void motores_cv_cambiar_velocidad(int velocidad)
{
   motores_cv_velocidad=velocidad;
}

void motores_cv_sum_velocidad(int velocidad)
{
   if ((motores_cv_velocidad+velocidad)<255 && (motores_cv_velocidad+velocidad)>0) motores_cv_velocidad+=velocidad;
   printf(usb_cdc_putc, "Vel: %u\r\n",  motores_cv_velocidad);
}
